import sys
sys.path.append('/home/mvccep/MVTP_ws_tm/src/one_car_planning_control/base/')
import hybrid_astar as h_astar # 混合A*算法
from Parameter import Parameter as PA # A*栅格地图参数、车辆尺寸参数

# 混合A*规划路径，输入：起点、终点（x, y, yaw）State类型； 输出：路径（list类型，每一个元素都是State类型）
def hybridAstarPathPlanning(startP, endP):
    """
    规划算法
    Args:startP (list)、endP (list)
    Returns:path 路径
    """
    # 起点终点预处理，转换为A*地图坐标 （实际场景中以矩形中点为坐标原点，A*地图中以左下角为坐标原点）
    startP[0] = startP[0] / PA.N + PA.MAP_SIZE[0] / 2
    startP[1] = startP[1] / PA.N + PA.MAP_SIZE[0] / 2
    endP[0] = endP[0] / PA.N + PA.MAP_SIZE[0] / 2
    endP[1] = endP[1] / PA.N + PA.MAP_SIZE[0] / 2
    
    # path是一个类实例，包含x, y, yaw, direction, cost属性，其中每个变量均为list
    path = h_astar.main(startP, endP) #调用混合A*进行路径规划
    
    # A*地图坐标转换为实际坐标（相对于预处理公式中的逆转换）
    for i in range(len(path.x)):
        path.x[i] = (path.x[i] - PA.MAP_SIZE[0]/2) * PA.N
        path.y[i] = (path.y[i] - PA.MAP_SIZE[1]/2) * PA.N
    
    return path
